#pragma once
#include <cmath>
#include "Simpson.h"
#include <vector>
#include <iostream>
#include "point.h"
#include "function.h"

class BezierCurve3 {
public:
	Point p0, p1, p2, p3;
	BezierCurve3(Point a, Point b, Point c, Point d) {
		p0 = a;
		p1 = b;
		p2 = c;
		p3 = d;
	}
	BezierCurve3() {}
    Point at(float t) {       //返回函数值
		return p0 * (pow(1 - t, 3)) + p1 * 3 * t * pow(1 - t,2) + p2 * 3 * pow(t, 2) * (1 - t) + p3 * pow(t, 3);
	}
    Point dev(float t) {      //一阶导数 对参数t
		Point k1 = (p3 - p2 * 3 + p1 * 3 - p0) * 3;
		Point k2 = (p2 - p1 * 2 + p0) * 6;
		Point k3 = (p1 - p0) * 3;
		return k1 * pow(t,2) + k2 * t + k3;
	}
    Point dev2(float t) {     //二阶导数 对参数t
		Point k1 = (p3 - p2 * 3 + p1 * 3 - p0) * 6;
		Point k2 = (p2 - p1 * 2 + p0) * 6;
		return k1 * t + k2;
	}
    float total_length() {
        auto df = [&](float t) -> float
		{
			return this->dev(t).norm();
		};
		return NumericalIntegration::adaptive_simpson_3_8(df, 0, 1);
	}
    float length_with_t(float t) {
        auto df = [&](float t) -> float
		{
			return this->dev(t).norm();
		};
		return NumericalIntegration::adaptive_simpson_3_8(df, 0, t);
	}
    float getTByArcLength_Newton(float target_length) { //首先是一个凸函数，使用Newton法获取弧长s时对应的t,初始点的选取很重要
        size_t max_iter_time = 30;
        float iter_eps = 0.0001;
        float approx_t = target_length / total_length();
        float prev_approx_t = 0;
		if (approx_t > 1 || approx_t < 0) {
			std::cout << "FILE:BezierCurve3.h LINE:100\n";
		}
        auto df = [&](float t) -> float
		{
			return this->dev(t).norm();
		};
		for (int iter = 0; iter < max_iter_time; ++iter)
		{
            float approx_length = NumericalIntegration::adaptive_simpson_3_8(df, 0, approx_t);
            float d = approx_length - target_length;
			if (abs(d) < iter_eps){
				std::cout << "The number of iterations is " << iter << std::endl;
				return approx_t;
			}

			// Newton's method
            float first_order = this->dev(approx_t).norm();
            float second_order = this->dev2(approx_t).norm();
            float numerator = d * first_order;
            float denominator = d * second_order + first_order * first_order;
			// update 
			approx_t = approx_t - numerator / denominator;

			if (abs(approx_t - prev_approx_t) < iter_eps) {
                std::cout << "The number of iterations is " << iter << std::endl;
				return approx_t;
			}
			else prev_approx_t = approx_t;
		}
		std::cout << "Did not converge within the specified number of times\n";
        return approx_t;
	}
    float getTByArcLength_Newton(float start,float target_length) { //首先是一个凸函数，使用Newton法获取弧长s时对应的t,初始点的选取很重要
        size_t max_iter_time = 30;
        float iter_eps = 0.0001;
        float approx_t = start + target_length / total_length();
        float prev_approx_t = 0;
        if (approx_t > 1 || approx_t < 0) {
            std::cout << "FILE:BezierCurve3.h LINE:100\n";
        }
        auto df = [&](float t) -> float
        {
            return this->dev(t).norm();
        };
        for (int iter = 0; iter < max_iter_time; ++iter)
        {
            float approx_length = NumericalIntegration::adaptive_simpson_3_8(df, start, approx_t);
            float d = approx_length - target_length;
            if (abs(d) < iter_eps){
                std::cout << "The number of iterations is " << iter << std::endl;
                return approx_t;
            }

            // Newton's method
            float first_order = this->dev(approx_t).norm();
            float second_order = this->dev2(approx_t).norm();
            float numerator = d * first_order;
            float denominator = d * second_order + first_order * first_order;
            // update
            approx_t = approx_t - numerator / denominator;

            if (abs(approx_t - prev_approx_t) < iter_eps) {
                std::cout << "The number of iterations is " << iter << std::endl;
                return approx_t;
            }
            else prev_approx_t = approx_t;
        }
        std::cout << "Did not converge within the specified number of times\n";
        return approx_t;
    }
    float getTByArcLength_Steffensen(float target_length) {
		size_t max_iter_time = 10;
        float iter_eps = 0.001;
        float approx_t = target_length / total_length();
        float prev_approx_t = 0;
		if (approx_t > 1 || approx_t < 0) {
            std::cout << "FILE:BezierCurve3.h LINE:100\n";
		}
        auto df = [&](float t) -> float
		{
			return this->dev(t).norm();
		};
		for (int iter = 0; iter < max_iter_time; ++iter)
		{
            float approx_length = NumericalIntegration::adaptive_simpson_3_8(df, 0, approx_t);
            float d = approx_length - target_length;
			if (abs(d) < iter_eps) {
				std::cout << "The number of iterations is " << iter << std::endl;
				return approx_t;
			}

            // Newton's method
            float first_order = this->dev(approx_t).norm();
            float numerator = pow(d, 2);
            float denominator = 2 * d * first_order;
            float temp_k1 = approx_t - numerator / denominator;

            // Newton's method
            approx_length = NumericalIntegration::adaptive_simpson_3_8(df, 0, temp_k1);
            d = approx_length - target_length;
            if (abs(d) < iter_eps) {
                return approx_t;
            }
            first_order = this->dev(temp_k1).norm();
            numerator = pow(d, 2);
            denominator = 2 * d * first_order;
            float temp_k2 = temp_k1 - numerator / denominator;

            numerator = (temp_k1 - approx_t) * (temp_k1 - approx_t);
            denominator = temp_k2 - 2 * temp_k1 + approx_t;
            approx_t = approx_t - numerator / denominator;
//			std::cout << "numerator = " << numerator << std::endl;
//			std::cout << "denominator = " << denominator << std::endl;
//			std::cout << "temp_k1 = " << temp_k1 << std::endl;
//			std::cout << "temp_k2 = " << temp_k2 << std::endl;
//			std::cout << "approx_t = " << approx_t << std::endl;
			if (abs(approx_t - prev_approx_t) < iter_eps) {
				std::cout << "The number of iterations is " << iter << std::endl;
				return approx_t;
			}
			else prev_approx_t = approx_t;
		}
		std::cout << "Did not converge within the specified number of times\n";
		return approx_t;
	}
    float getTByArcLength_Steffensen(float start,float target_length) {
        size_t max_iter_time = 10;
        float iter_eps = 0.0001;
        auto df = [&](float t) -> float
        {
            return this->dev(t).norm();
        };
        float approx_t = start + target_length / total_length();
        //std::cout << "approx_t_1 = " << approx_t << std::endl;
        float b = NumericalIntegration::adaptive_simpson_3_8(df, start, approx_t) - target_length;
        float k = 2 * b * this->dev(approx_t).norm();
        if(k != 0)
        approx_t = -b/k + approx_t;
        //std::cout << "approx_t_2 = " << approx_t << std::endl;
        float prev_approx_t = 0;
        if (approx_t > 1 || approx_t < 0) {
        }

        for (int iter = 0; iter < max_iter_time; ++iter)
        {
            float approx_length = NumericalIntegration::adaptive_simpson_3_8(df, start, approx_t);
            float d = approx_length - target_length;
            if (abs(d) < iter_eps) {
                //std::cout << "The number of iterations is " << iter << std::endl;
                return approx_t;
            }

            // Newton's method
            float first_order = this->dev(approx_t).norm();
            float numerator = pow(d, 2);
            float denominator = 2 * d * first_order;
            float temp_k1 = approx_t - numerator / denominator;

            // Newton's method
            approx_length = NumericalIntegration::adaptive_simpson_3_8(df, start, temp_k1);
            d = approx_length - target_length;
            if (abs(d) < iter_eps) {
                return approx_t;
            }
            first_order = this->dev(temp_k1).norm();
            numerator = pow(d, 2);
            denominator = 2 * d * first_order;
            float temp_k2 = temp_k1 - numerator / denominator;

            //Steffensen accelerate
            numerator = (temp_k1 - approx_t) * (temp_k1 - approx_t);
            denominator = temp_k2 - 2 * temp_k1 + approx_t;
            approx_t = approx_t - numerator / denominator;
            if (abs(approx_t - prev_approx_t) < iter_eps) {
                //std::cout << "The number of iterations is " << iter << std::endl;
                return approx_t;
            }
            else prev_approx_t = approx_t;
        }
        std::cout << "Did not converge within the specified number of times\n";
        return approx_t;
    }
    float getCurvature(const float& t) { //当曲率小于一定值时，认为沿直线前进
        float yz = pow(dev2(t).m_z * dev(t).m_y - dev(t).m_z * dev2(t).m_y, 2);
        float xz = pow(dev2(t).m_x * dev(t).m_z - dev(t).m_x * dev2(t).m_z, 2);
        float xy = pow(dev2(t).m_y * dev(t).m_x - dev(t).m_y * dev2(t).m_x, 2);

        float numerator = sqrt(yz + xy + xz);
        float denominator = pow(dev(t).m_x * dev(t).m_x + dev(t).m_y * dev(t).m_y + dev(t).m_z * dev(t).m_z, 1.5);
        if (denominator < 1e-6)
            return -1; //表示此处曲率无穷大
        else
        return numerator / denominator;
    }
};
